#include "StdAfx.h"
#include "fitting.h"


struct data {
       size_t n;
       double * y;
       double * sigma;
	   fitting *ref;
     };

fitting::fitting(fit_modul modul)
{
	this->modul=&modul;
	this->param.clear();
	for(int i=0;i<modul.getparamcount();++i){
		this->param.resize(this->param.size()+1);
		this->param.at(this->param.size()-1).fit=false;
		this->param.at(this->param.size()-1).name=modul.param.at(i).name;
		this->param.at(this->param.size()-1).value=modul.param.at(i).value;
	}
}


fitting::~fitting(void)
{
}

void fitting::setModul(fit_modul *modul){
	this->modul=modul;
	this->param.clear();
	for(int i=0;i<modul->getparamcount();++i){
		this->param.resize(this->param.size()+1);
		this->param.at(this->param.size()-1).fit=false;
		this->param.at(this->param.size()-1).name=modul->param.at(i).name;
		this->param.at(this->param.size()-1).value=modul->param.at(i).value;
	}
	if(this->filename.size()>0){
		this->modul->filename=this->filename;
		
	}
}

void fitting::set_param_value(int i, double value){
	if(this->param.size()>i){
		this->param.at(i).value=value;
	}
}
void fitting::set_param_fit(int i, bool fit){
	if(this->param.size()>i){
		this->param.at(i).fit=fit;
		this->modul->param.at(i).fit=fit;
	}
}


void fitting::loadData(std::string filename){
	this->filename=filename;
	if(this->param.size()>0){
		this->modul->filename=filename;
	}
	std::fstream datei;
	std::vector<std::string> line;
	std::string temp;
	datei.open(filename, std::ios::in);
	while(!datei.eof()){
		getline(datei,temp);
		line.push_back(temp);
	}
	datei.close();
	this->xdata.clear();
	this->ydata.clear();
	std::size_t pos;
	std::string xtemp;
	std::string ytemp;
	for(int i=0;i<line.size();++i){
		//pos=line.at(i).find(" ");
		if(line.at(i).find(" ")!=std::string::npos){
			pos=line.at(i).find(" ");
			xtemp=line.at(i).substr(0,pos);
			ytemp=line.at(i).substr(pos);
			this->xdata.push_back(atof(xtemp.c_str()));
			this->ydata.push_back(atof(ytemp.c_str()));
		}else if (line.at(i).find("\t")!=std::string::npos){
			pos=line.at(i).find("\t");
			xtemp=line.at(i).substr(0,pos);
			ytemp=line.at(i).substr(pos);
			this->xdata.push_back(atof(xtemp.c_str()));
			this->ydata.push_back(atof(ytemp.c_str()));
		}else{
			//this->xdata.push_back(1337);
			//this->ydata.push_back(1337);
		}
	}
}

int fitting::get_paramcount()
{
	int tempcount=0;
	for(int i=0;i<this->param.size();++i){
		if(this->param.at(i).fit){
			++tempcount;
		}
	}
	return tempcount;
}

std::vector<fitting::parameter> fitting::get_param(){
	return this->param;
}



int func_f(const gsl_vector* x,void *data, gsl_vector * f){
	size_t n = ((struct data *)data)->n;
       double *y = ((struct data *)data)->y;
       double *sigma = ((struct data *) data)->sigma;
	   fitting *thisref=((struct data *) data)->ref;

	   std::vector<double> tempparam;
	   for (int i=0;i<thisref->get_paramcount();++i){
		   tempparam.push_back(gsl_vector_get(x,i));
	   }
	   std::vector<double> par;
	   int k=0;
	   for(int i=0;i<thisref->get_param().size();++i){
		   if(thisref->get_param().at(i).fit){
			   par.push_back(tempparam.at(k));
			   ++k;
		   }else{
			   par.push_back(thisref->get_param().at(i).value);
		   }
	   }
	   thisref->modul->setparams(par);
	   std::vector<double> ytemp=thisref->modul->f();

	  

	   for(int i=0;i<ytemp.size();++i){
		    gsl_vector_set(f,i,((ytemp.at(i)-y[i])/sigma[i]));
	   }

	return GSL_SUCCESS;
}

int func_df(const gsl_vector * x, void *data, gsl_matrix * J){
	size_t n = ((struct data *)data)->n;
       double *y = ((struct data *)data)->y;
       double *sigma = ((struct data *) data)->sigma;
	   fitting *thisref=((struct data *) data)->ref;

	   std::vector<double> tempparam;
	   for (int i=0;i<thisref->get_paramcount();++i){
		   tempparam.push_back(gsl_vector_get(x,i));
	   }
	   std::vector<double> par;
	   int k=0;
	   for(int i=0;i<thisref->get_param().size();++i){
		   if(thisref->get_param().at(i).fit){
			   par.push_back(tempparam.at(k));
			   ++k;
		   }else{
			   par.push_back(thisref->get_param().at(i).value);
		   }
	   }
	   thisref->modul->setparams(par);
	
	   

	   std::vector<std::vector<double> > xy=thisref->modul->df();

	   

	   if(xy.size()!=0){
	   for(int i=0;i<xy.size();++i){
		   for(int k=0;k<xy.at(i).size();++k){
			   gsl_matrix_set(J,i,k,xy.at(i).at(k));
		   }
	   }
	   }else{
		   J=NULL;
	   }

	return GSL_SUCCESS;
}

int func_fdf (const gsl_vector * x, void *data,gsl_vector * f, gsl_matrix * J){
	size_t n = ((struct data *)data)->n;
       double *y = ((struct data *)data)->y;
       double *sigma = ((struct data *) data)->sigma;
	   fitting *thisref=((struct data *) data)->ref;

	   std::vector<double> tempparam;
	   for (int i=0;i<thisref->get_paramcount();++i){
		   tempparam.push_back(gsl_vector_get(x,i));
	   }
	   std::vector<double> par;
	   int k=0;
	   for(int i=0;i<thisref->get_param().size();++i){
		   if(thisref->get_param().at(i).fit){
			   par.push_back(tempparam.at(k));
			   ++k;
		   }else{
			   par.push_back(thisref->get_param().at(i).value);
		   }
	   }
	   thisref->modul->setparams(par);

	   fit_modul::vecma temp=thisref->modul->fdf();

	  

	   for(int i=0;i<temp.f.size();++i){
		  // std::cout<<(temp.f.at(i)-y[i])/sigma[i]<<"  "<<sigma[i]<<std::endl;
		   gsl_vector_set(f,i,((temp.f.at(i)-y[i])/sigma[i]));
	   }
	   if(temp.df.size()!=0){
	   for(int i=0;i<temp.df.size();++i){
		   for(int k=0;k<temp.df.at(i).size();++k){
			   gsl_matrix_set(J,i,k,temp.df.at(i).at(k));
		   }
	   }
	   }else{
		J=NULL;
	   }
	return GSL_SUCCESS;
}

void fitting::print_state (size_t iter, gsl_multifit_fdfsolver * s,std::fstream &logfile)
{
	
	logfile<<"iter: "<<" x ="<<iter<<'\t';
	printf ("iter: %3u x =",iter);

	for(int i=0;i<this->get_paramcount();++i){
		logfile<<gsl_vector_get (s->x, i)<<' \t ';
		printf(" % 15.8f",gsl_vector_get (s->x, i));
	}
	logfile<<"|f(x)| = "<<gsl_blas_dnrm2 (s->f)<<std::endl;
	printf(" |f(x)| = %g\n",gsl_blas_dnrm2 (s->f));

	
}

int fitting::fit(){
	
	//std::freopen("fitlog.txt","w",stdout);
	
	std::fstream logfile;
	logfile.open("Fitlog.txt",std::ios::out);

	if(this->xdata.size()>0 && this->ydata.size()>0){
		this->modul->xdata=this->xdata;
		this->modul->ydata=this->ydata;
	}else{
	return 1; //abort this shit
	}


	const gsl_multifit_fdfsolver_type *T;
	gsl_multifit_fdfsolver *s;
	int status;
	unsigned int i, iter = 0;
	const size_t n = this->xdata.size();
	size_t p = this->get_paramcount();

	gsl_matrix *covar = gsl_matrix_alloc (p, p);
	
	double *y=0;
	double *sigma=0;
	y=new double [this->xdata.size()];
	sigma=new double [this->xdata.size()];
	struct data d = { n, y, sigma, this};
	gsl_multifit_function_fdf f;

	
	double *x_init;
	x_init=new double[this->get_paramcount()];
	int i2=0;
	for(i=0;i<this->param.size();++i){
		if(this->param.at(i).fit){
			x_init[i2]=param.at(i).value;
			++i2;
		}
	}
	gsl_vector_view x = gsl_vector_view_array (x_init, p);

	f.f = &func_f;
	f.df = &func_df;
	f.fdf = &func_fdf;
	f.n = n;
	f.p = p;
	f.params = &d;
	
	for (i = 0; i < n; i++)
	{
		y[i]=this->ydata.at(i);
		
		sigma[i]=errabs+this->ydata.at(i)*errrel;
		//std::cout<<sigma[i]<<std::endl;
	}

	T = gsl_multifit_fdfsolver_lmsder;
	s = gsl_multifit_fdfsolver_alloc (T, n, p);
	gsl_multifit_fdfsolver_set (s, &f, &x.vector);
	print_state (iter, s,logfile);

	do
	{
		iter++;
		status = gsl_multifit_fdfsolver_iterate (s);
		logfile<<"status = "<<gsl_strerror (status)<<std::endl;
		printf ("status = %s\n", gsl_strerror (status));
		print_state (iter, s,logfile);
		if (status)
		break;
		status = gsl_multifit_test_delta (s->dx, s->x,1e-5, 1e-5);

		/*gsl_vector * grad=gsl_vector_alloc(p);
		
		gsl_multifit_gradient ( s->J, s->f, grad);
		status = gsl_multifit_test_gradient ( grad, 1e-4);*/


	}while (status == GSL_CONTINUE && iter < 500);


	


	gsl_multifit_covar (s->J, 0.0, covar);
	
	//sorting parameters
	int i3=0;
	for(int i=0;i<this->param.size();++i){
		if(this->param.at(i).fit){
			this->param.at(i).value=gsl_vector_get(s->x, i3);
			++i3;
		}
	}
	//calculatin chisq and errors
	#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))
	{
	double chi = gsl_blas_dnrm2(s->f);
	double dof = n - p;
	double c = GSL_MAX_DBL(1, chi / sqrt(dof));
	logfile<<"chisq/dof ="<<pow(chi, 2.0) / dof<<std::endl;
	printf("chisq/dof = %g\n", pow(chi, 2.0) / dof);
	for(int i=0;i<this->param.size();++i){
		if(this->param.at(i).fit){
			printf(this->param.at(i).name.c_str());
			logfile<<this->param.at(i).name.c_str()<<" = "<<gsl_vector_get(s->x, i)<<" +/-"<<c*ERR(i)<<std::endl;
			printf (" = %.5f +/- %.5f\n", gsl_vector_get(s->x, i), c*ERR(i));
		}else{
			printf(this->param.at(i).name.c_str(),logfile);
			printf(this->param.at(i).name.c_str());
			logfile<<this->param.at(i).name.c_str()<<" = "<<this->param.at(i).value<<std::endl;
			printf (" = %.5f\n", this->param.at(i).value);
		}
	}
	
	}

	
	logfile<<"status = "<<gsl_strerror (status)<<std::endl;
	printf ("status = %s\n", gsl_strerror (status));



	//free memory
	gsl_multifit_fdfsolver_free (s);
	gsl_matrix_free (covar);
	

	delete[] y;			
	y=0;
	delete[] sigma;
	sigma=0;
	delete[] x_init;
	x_init=0;
	
	logfile.close();
		return 0;
	}

void fitting::getPrameter(){
	std::vector<fit_modul::parameter> par=this->modul->getparams();
	fitting::parameter temp;
	for(int i=0;i<par.size();++i){
		temp.value=par.at(i).value;
		temp.name=par.at(i).name;
		temp.fit=true;
		this->param.push_back(temp);
	}
	this->paramcount=this->param.size();

}

